/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.propagation;

import ore.Attitude;
import ore.Orbit;
import ore.SpacecraftState;
import ore.Transform;
import ore.orbits.EquinoctialOrbit;
import ore.time.AbsoluteDate;
import ore.time.ChronologicalComparator;
import ore.time.TimeStamped;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;

import java.util.Iterator;
import java.util.SortedSet;
import java.util.TreeSet;

/** 
 * This class is designed to accept and handle tabulated orbital
 * entries.  Tabulated entries are classified and then extrapolated to
 * obtain continuous output, with accuracy and computation methods
 * configured by the user.
 *
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class Ephemeris 
    extends Object
    implements BoundedPropagator
{

    /** 
     * All entries.
     */
    private final SortedSet<TimeStamped> data;
    /** 
     * Previous state in the cached selection.
     */
    private SpacecraftState previous;
    /** 
     * Next state in the cached selection.
     */
    private SpacecraftState next;


    /** 
     * @param tabulatedStates States table
     */
    public Ephemeris(SpacecraftState[] tabulatedStates) {
        super();
        if (tabulatedStates.length < 2)
            throw new IllegalArgumentException("There should be at least 2 entries.");
        else {
            this.data = new TreeSet<TimeStamped>(new ChronologicalComparator());
            for (int i = 0; i < tabulatedStates.length; ++i) {
                data.add(tabulatedStates[i]);
            }
        }
    }


    /** 
     * @return The first date of the range
     */
    public AbsoluteDate getMinDate() {
        return this.data.first().getDate();
    }
    /** 
     * @return The last date of the range
     */
    public AbsoluteDate getMaxDate() {
        return this.data.last().getDate();
    }
    /**
     * {@inheritDoc}
     */
    public SpacecraftState propagate(AbsoluteDate date) {
        /*
         * Check if date is in the specified range
         */
        if (this.enclosinbracketDate(date)) {

            final double tp = date.durationFrom(previous.getDate());
            final double tn = next.getDate().durationFrom(date);
            if (tp == 0 && tn == 0)
                return this.previous;
            else {
                /*
                 * Classical interpolation
                 */
                return new SpacecraftState(this.getInterpolatedOp(tp, tn, date),
                                           this.interpolatedAttitude(tp, tn),
                                           this.interpolatedMass(tp, tn));
            }
        }
        else {
            /*
             * Outside date range
             */
            return null;
        }
    }
    /** 
     * Get the interpolated orbital parameters.
     *
     * @param tp Time in seconds since previous date
     * @param tn Time in seconds until next date
     * @param date Desired date for the state
     *
     * @return The new equinoctial parameters
     */
    private Orbit getInterpolatedOp(double tp, double tn, AbsoluteDate date) {

        final double dt = (tp + tn);
        final double cP = (tp / dt);
        final double cN = (tn / dt);

        final double a  = ((cN * previous.getA())  + (cP * next.getA()));
        final double ex = ((cN * previous.getEquinoctialEx()) + (cP * next.getEquinoctialEx()));
        final double ey = ((cN * previous.getEquinoctialEy()) + (cP * next.getEquinoctialEy()));
        final double hx = ((cN * previous.getHx()) + (cP * next.getHx()));
        final double hy = ((cN * previous.getHy()) + (cP * next.getHy()));
        final double lv = ((cN * previous.getLv()) + (cP * next.getLv()));

        return new EquinoctialOrbit(a, ex, ey, hx, hy, lv,
                                    EquinoctialOrbit.TRUE_LATITUDE_ARGUMENT,
                                    this.previous.getFrame(), date, this.previous.getMu());
    }
    /** 
     * Get the interpolated Attitude.
     * 
     * @param tp Time in seconds since previous date
     * @param tn Time in seconds until next date
     * 
     * @return The new attitude kinematics
     */
    private Attitude interpolatedAttitude(double tp, double tn) {

        final double dt = (tp + tn);

        final Transform prevToNext =
            new Transform(new Transform(this.previous.getAttitude().getRotation().revert()),
                          new Transform(this.next.getAttitude().getRotation()));

        final Rotation newRot = new Rotation(prevToNext.getRotation().getAxis(),
                                             ((tp * prevToNext.getRotation().getAngle()) / dt));
        Vector3D newInstRotAxis;
        if (prevToNext.getRotationRate().getNorm() != 0) {
            newInstRotAxis = new Vector3D(((tp * prevToNext.getRotationRate().getNorm()) / dt),
                                          prevToNext.getRotationRate().normalize());
        }
        else {
            newInstRotAxis = Vector3D.ZERO;
        }

        final Transform newTrans =
            new Transform(new Transform(this.previous.getAttitude().getRotation()),
                          new Transform(newRot, newInstRotAxis));

        return new Attitude(this.previous.getFrame(), newTrans.getRotation(), newTrans.getRotationRate());
    }
    /** 
     * Get the interpolated Mass.
     * 
     * @param tp Time in seconds since previous date
     * @param tn Time in seconds until next date
     * 
     * @return The new mass
     */
    private double interpolatedMass(double tp, double tn) {
        return ((tn * previous.getMass()) + (tp * next.getMass())) / (tn + tp);
    }
    /** 
     * Find the states bracketing a date.
     * 
     * @param date Date to bracket
     * 
     * @return Bracketing states have been found
     */
    private boolean enclosinbracketDate(AbsoluteDate date) {

        if (date.durationFrom(getMinDate()) < 0 || date.durationFrom(getMaxDate()) > 0) {
            return false;
        }
        else if (date.durationFrom(getMinDate()) == 0) {
            this.previous = (SpacecraftState) this.data.first();
            final Iterator<TimeStamped> i = this.data.iterator();
            i.next();
            this.next = (SpacecraftState) i.next();
            return true;
        }
        else {
            /*
             * Don't search if the cached selection is fine
             */
            if ((this.previous != null) && (date.durationFrom(this.previous.getDate()) >= 0) &&
                (this.next != null) && (date.durationFrom(this.next.getDate()) < 0))
            {
                /*
                 * The current selection is already good
                 */
                return true;
            }
            else {
                /*
                 * Search bracketing states
                 */
                this.previous = (SpacecraftState) this.data.headSet(date).last();
                this.next     = (SpacecraftState) this.data.tailSet(date).first();

                return true;
            }
        }
    }
}
